﻿// InsGpsEulerDirectEKF.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#define _CRT_SECURE_NO_DEPRECATE
#include "EKF.h"
#include "main.h"
#include <time.h>

using namespace matrix;
std::vector<UavSensors> vUavSensors;
clsInsGpsLooseEulerDirectEKF<DataType, 15, 1, 6> InsGpsLooseEulerDirectEKF;
DataType pQt_data[15] = {4.00000000000000e-03, 4.00000000000000e-03, 4.00000000000000e-03,
						 2.50000000000000e-08, 2.50000000000000e-08, 2.50000000000000e-08,
						 1.00000000000000e-06, 1.00000000000000e-06, 1.00000000000000e-06,
						 1.00000000000000e-06, 1.00000000000000e-06, 1.00000000000000e-06,
						 1.00000000000000e-06, 1.00000000000000e-06, 1.00000000000000e-06};
int Qt_dim = 15;
DataType pR_yaw_data[1] = {0.0200000000000000};
int Ryaw_dim = 1;
DataType pR_gps_data[6] = {0.0001, 0.0001, 0.0001,
						   0.25, 0.25, 0.25};
int Rgps_dim = 6;
DataType outcome[15] = {0};
int outcome_dim = 15;
/*
int main() //测试函数
{
	char datapath[100];
	sprintf(datapath, "./DATA/TEST1.txt");
	//vUavSensors.clear();
	prepare_data(datapath);
	for (uint32_t i = 0; i < vUavSensors.size(); i++)
	{
		receive(time_s, gx_rps, gy_rps, gz_rps,
				fx_mps2, fy_mps2, fz_mps2,
				isGPSUpdate, Pn_m, Pe_m, h_msl_m);
	}
	return 0;
}*/


void EKF_Update(EKF_RawData* EKFRAW)
{
	static int firstcall=1;
	static double time_spre = EKFRAW->TimeStamp;
	static double GNGGA_latitude0 = EKFRAW->GNGGA_latitude;
	static double GNGGA_longtitude0 = EKFRAW->GNGGA_longtitude;
	static double gps_pos_npre = 0;
	static double gps_pos_epre = 0;
	static double gps_pos_hpre = 0;
	static double cos_latitude0 = cos(GNGGA_latitude0 * pi / 180);
	static int timebuff=0;
	if(firstcall && EKFRAW->isGPSUpdate==0)
		return ;
	
	if(firstcall &&EKFRAW->isGPSUpdate==1)
	{
		GNGGA_latitude0 = EKFRAW->GNGGA_latitude;
		GNGGA_longtitude0 = EKFRAW->GNGGA_longtitude;
	}
	double gps_pos_n = 0;
	double gps_pos_e = 0;
	double gps_pos_h = 0;
	if(EKFRAW->isGPSUpdate)
	{
		gps_pos_n = (EKFRAW->GNGGA_latitude- GNGGA_latitude0) * 60 * 60 * 30.9;
		gps_pos_e = (EKFRAW->GNGGA_longtitude- GNGGA_longtitude0) * 60 * 60 * 30.8 * cos_latitude0;
		gps_pos_h = EKFRAW->GNGGA_height;
	}
	
	if(firstcall)
	{
		firstcall=0;
		gps_pos_npre = gps_pos_n;
		gps_pos_epre = gps_pos_e;
		gps_pos_hpre = gps_pos_h;
	}
	gps_pos_n +=timebuff;
	gps_pos_e += timebuff;
	timebuff++;
	double dt = (EKFRAW->TimeStamp - time_spre)/1000.0f;
	if(dt==0)
		dt=0.2;
	time_spre = EKFRAW->TimeStamp;
	
	//printf("[gps_pos]%.2f,%.2f\n",gps_pos_n,EKFRAW->GNGGA_latitude);
	double gps_vel_n = (gps_pos_n - gps_pos_npre) / dt;
	double gps_vel_e = (gps_pos_e - gps_pos_epre) / dt;
	double gps_vel_h = (gps_pos_h - gps_pos_hpre) / dt;
	gps_pos_npre = gps_pos_n;
	gps_pos_epre = gps_pos_e;
	gps_pos_hpre = gps_pos_h;
	
	int isYawUpdate = 0;
	double yaw_rad = 0;
	printf("[EKF]%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",gps_pos_n, gps_pos_e, gps_pos_h, gps_vel_n, gps_vel_e, gps_vel_h);
	InsGpsLooseEulerDirectEKF.Run(
		outcome, outcome_dim,														   // 滤波结果
		dt,		
			
		EKFRAW->gx_rps, EKFRAW->gy_rps, EKFRAW->gz_rps,														   // 陀螺仪
		EKFRAW->fx_mps2, EKFRAW->fy_mps2, EKFRAW->fz_mps2,		
		
		isYawUpdate, yaw_rad,														   // 航向角测量值
		EKFRAW->isGPSUpdate, 
		gps_pos_n, gps_pos_e, gps_pos_h, 
		gps_vel_n, gps_vel_e, gps_vel_h, // GPS测量值
		pQt_data, Qt_dim,															   // Q 模型噪声描述
		pR_yaw_data, Ryaw_dim,														   // R_yaw 航向角测量方差描述
		pR_gps_data, Rgps_dim														   // R_gps 位置速度测量方差描述
	);
	EKFRAW -> EKF_POS_N = outcome[3]/ 60.0f / 60.0f / 30.9f + GNGGA_latitude0;
	EKFRAW -> EKF_POS_E = outcome[4] /60.0f / 60.0f / 30.8f / cos_latitude0 + GNGGA_longtitude0;
}

int prepare_data(const char *datapath)
{
	// 准备数据：(MATLAB)在DATA文件夹里运行generate_uav_sensors_measurement_for_cpp.m文件，生成 UavData_SquareSpiral.txt
	// 读取数据
	std::ifstream f_read(datapath);
	//std::ifstream f_read("./DATA/UavData_SquareSpiral.txt");
	if (!f_read)
	{
		std::cout << "ifstream - Cannot open " << datapath << std::endl;
		std::cout << "Exit :(" << std::endl;
		return -1;
	}
	std::string str_fread;
	while (getline(f_read, str_fread))
	{
		//cout << str_fread << endl;
		// 把逗号换成空格
		//for(std::string::iterator iter = str_fread.begin(); iter!=str_fread.end();iter++)
		for (uint32_t i = 0; i < str_fread.length(); i++)
		{
			if (str_fread.at(i) == ',')
				str_fread.at(i) = ' ';
			//if (*iter == ',') str_fread = ' ';
		}
		//cout << str_fread << endl;
		// 提取数字（基于istringstream，头文件<sstream>）
		std::istringstream out(str_fread);
		std::string tmp;
		struct UavSensors uavSensors;
		while (out >> tmp)
		{
			// 将字符串转换为数字 stoi stof stod
			static int stage = 0; // 设定 stage 控制流程
			if (stage == 0)
			{
				uavSensors.time_s = stod(tmp); // time_s
				stage++;
				continue;
			}
			if (stage == 1)
			{
				uavSensors.gx_rps = stod(tmp);
				stage++;
				continue;
			}
			if (stage == 2)
			{
				uavSensors.gy_rps = stod(tmp);
				stage++;
				continue;
			}
			if (stage == 3)
			{
				uavSensors.gz_rps = stod(tmp);
				stage++;
				continue;
			}
			if (stage == 4)
			{
				uavSensors.fx_mps2 = stod(tmp);
				stage++;
				continue;
			}
			if (stage == 5)
			{
				uavSensors.fy_mps2 = stod(tmp);
				stage++;
				continue;
			}
			if (stage == 6)
			{
				uavSensors.fz_mps2 = stod(tmp);
				stage++;
				continue;
			}
			if (stage == 7)
			{
				uavSensors.isYawUpdate = stoi(tmp);
				stage++;
				continue;
			}
			if (stage == 8)
			{
				uavSensors.yaw_deg = stod(tmp);
				//uavSensors.yaw_rad = uavSensors.yaw_deg * 3.1415926535f / 180.0f;
				stage++;
				continue;
			}
			if (stage == 9)
			{
				uavSensors.isGPSUpdate = stoi(tmp);
				stage++;
				continue;
			}
			if (stage == 10)
			{
				uavSensors.Pn_m = stod(tmp);
				//uavSensors.Pn_m = (uavSensors.Pn_m - I0) * 3600.0f * 30.9f;
				stage++;
				continue;
			}
			if (stage == 11)
			{
				uavSensors.Pe_m = stod(tmp);
				//uavSensors.Pe_m = (uavSensors.Pe_m - B0) * 3600.0f * 30.8f * cos(I0);
				stage++;
				continue;
			}
			if (stage == 12)
			{
				uavSensors.h_msl_m = stod(tmp);
				stage = 0;
				//continue;
			}

			//printf("%f,%f,%f,%f,%f,%f,%f,%d,%f,%d,%f,%f,%f,%f,%f,%f\n",
			//uavSensors.time_s,
			//uavSensors.gx_rps, uavSensors.gy_rps, uavSensors.gz_rps,
			//uavSensors.fx_mps2, uavSensors.fy_mps2, uavSensors.fz_mps2,
			//uavSensors.isYawUpdate, uavSensors.yaw_rad,
			//uavSensors.isGPSUpdate, uavSensors.Pn_m, uavSensors.Pe_m, uavSensors.h_msl_m, uavSensors.Vn_mps, uavSensors.Ve_mps, uavSensors.Vd_mps);
			// 将数据缓存至vector中
			vUavSensors.push_back(uavSensors);
		}
	}
	f_read.close();
	std::cout << "Total count: " << vUavSensors.size() << std::endl;
	/*for (int i = 0; i < vUavSensors.size(); i++)
	{
		printf("time_s=%f\t,%f\t,%f\t,%f\t,%f\t,%f\t,%f\t,%d\t,%f\t,%d\t,%f\t,%f\t,%f\t,%d\t,%d\t,%d\t\n",
			vUavSensors.at(i).time_s,
			vUavSensors.at(i).gx_rps, vUavSensors.at(i).gy_rps, vUavSensors.at(i).gz_rps,
			vUavSensors.at(i).fx_mps2, vUavSensors.at(i).fy_mps2, vUavSensors.at(i).fz_mps2,
			vUavSensors.at(i).isYawUpdate, vUavSensors.at(i).yaw_rad,
			vUavSensors.at(i).isGPSUpdate, vUavSensors.at(i).Pn_m, vUavSensors.at(i).Pe_m, vUavSensors.at(i).h_msl_m, vUavSensors.at(i).Vn_mps, vUavSensors.at(i).Ve_mps, vUavSensors.at(i).Vd_mps);
	}*/
	return 0;
}
